package de.tum.in.far.threedui.superbrickball.engine.physics;

import javax.media.j3d.Transform3D;
import javax.vecmath.Point3f;
import javax.vecmath.Quat4f;
import javax.vecmath.Vector3f;

public class Nature {
	public static Vector3f collide(Vector3f normal, Vector3f velocity)
	{
		Vector3f a = new Vector3f(velocity);
		Vector3f b = new Vector3f(normal);
		//get double the inverted projection of velocity on normal

		float ab = a.x*b.x + a.y * b.y + a.z * b.z;
		
		Vector3f p = new Vector3f(b);
		
		p.scale(ab/b.lengthSquared());
		
		p.scale(-2);
		
		
		//add to velovity
		velocity.add(p);
		
		return velocity;
	}
	
	public static Vector3f move(Vector3f velocity, Vector3f displacement)
	{
		displacement.add(velocity);
		return displacement;
	}
	
	public static Vector3f affect(Vector3f force, Vector3f velocity)
	{
		velocity.add(force);
		return velocity;
	}
	
	public static Vector3f getForce(Vector3f magnetVector, float magnitism, Vector3f targetVector)
	{
		Vector3f force = new Vector3f(targetVector);
		force.sub(magnetVector);
		force.normalize();
		force.scale(magnitism);
		return force;
	}
	
	public static Vector3f getForce(Transform3D magnet, float magnitism, Vector3f targetVector)
	{
		Vector3f translation = new Vector3f();
		Quat4f rotation = new Quat4f();
		magnet.get(translation);
		magnet.get(rotation);

		Vector3f normal = new Vector3f(0, 0, 1);
		Transform3D rotationT3D = new Transform3D();
		rotationT3D.set(rotation);
		rotationT3D.transform(normal);
		float d =CollisionDetector.distance(translation, targetVector);
		
		float scale = (float)Math.pow(d, 1.60);
		Vector3f magnetForce = new Vector3f(normal);
		magnetForce.scale(magnitism/ scale);

		//get normal distance between magnet line of effect and target
		Point3f X1 = new Point3f(translation);
		Point3f X2 = new Point3f(translation);
		X2.add(normal);
		Point3f X0 = new Point3f(targetVector);
		
		Point3f X1mX0 = sub(X1,X0);
		Point3f X2mX1 = sub(X2,X1);
		float ndNumerator = normSquare( X1mX0) * normSquare(X2mX1) - (float)Math.pow(dot(X1mX0, X2mX1), 2);
		float nd = ndNumerator / (normSquare(X2mX1));
		
		if(nd > 0.0012f)
		{
			magnetForce.set(0,0,0);
		}
//		System.out.printf("ND :[%f], FORCE :[%f]\n", nd , magnetForce.length());
		
		return magnetForce;
	}
	
	private static Point3f sub(Point3f p1, Point3f p2)
	{
		Point3f p = new Point3f(p1);
		p.sub(p2);
		return p;
	}
	
	private static Point3f add(Point3f p1, Point3f p2)
	{
		Point3f p = new Point3f(p1);
		p.add(p2);
		return p;
	}
	
	private static float dot(Point3f p1, Point3f p2)
	{
		return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;
	}
	
	private static float normSquare(Point3f p1)
	{
		return p1.x * p1.x + p1.y * p1.y + p1.z * p1.z;
	}
 

}
